﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ImageProcessingLibrary;

namespace HubertTestProgram
{
    public class DetectMovingPerson : BrainProcess
    {
        #region Fields
        private Bitmap webCamProcessedImage;
        private ImageProcessor webCamCurrentlyProcessingImage;
        private ImageProcessor backgroundImage;

        private bool personDetected = false;
        private double direcectionToPerson;
        private double cameraDirectionCurrentImage;
        private int pixelMiddleOfLargestComponent;
        #endregion
        
        #region Constants
        const int THRESHOLD_CONNECTED_COMPONENT = 200;
        #endregion

        #region Private methods
        private ImageProcessor ConvertToGrayScale(Bitmap inputBitmap)
        {
            ImageProcessor imageProcessor = new ImageProcessor(inputBitmap);
            imageProcessor.Lock();
            imageProcessor.ConvertToStandardGrayScale();
            imageProcessor.UnLock();
            return imageProcessor;
        }

        // Frame diff with running average (7.24 & 7.25)
        private ImageProcessor FrameDiffRunningAverage(ImageProcessor forground)
        {
            // Create new Background if nonexistant.
            if (this.backgroundImage == null)
            {
                this.backgroundImage = forground.Copy();
            }

            this.backgroundImage.Lock();
            forground.Lock();

            ImageProcessor diff = ImageProcessor.FrameDiffRunningAverage(forground, this.backgroundImage);
            diff.BinarizeGray(127, 255);
            diff.RemoveSinglePixels();
            diff.UnLock();
            this.backgroundImage.UnLock();
            forground.UnLock();
            return diff;
        }
        #endregion

        #region Public methods
        public override void Initialize()
        {
            base.Initialize();
            this.Utility = -1;
        }

        public override void Step()
        {
            if (CurrentState == 0)
            {
                this.backgroundImage = null;
                CurrentState = 1;
                
            }
            else if (CurrentState == 1)
            {
                Bitmap image = Robot.Instance.Body.WebCam.ReadingAsBitmap;
                if (image != null)
                {                    
                    webCamCurrentlyProcessingImage = ConvertToGrayScale(image);
                    this.cameraDirectionCurrentImage = Robot.Instance.Brain.Movement.EstimatedCurrentAbsoluteCameraPose;
                    CurrentState = 2;
                }
            }
            else if (CurrentState == 2)
            {
                webCamCurrentlyProcessingImage = FrameDiffRunningAverage(webCamCurrentlyProcessingImage);
                CurrentState=3;
            }
            else if (CurrentState == 3)
            {
                webCamCurrentlyProcessingImage.Lock();
                int sizeOfLargestComponent = webCamCurrentlyProcessingImage.ConnectedComponents();
                lock (this)
                {
                    this.pixelMiddleOfLargestComponent = webCamCurrentlyProcessingImage.PixelMiddleOfLargestComponent;
                    webCamCurrentlyProcessingImage.UnLock();
                    webCamProcessedImage = webCamCurrentlyProcessingImage.ProcessedBitmap;
                }

                if (sizeOfLargestComponent > THRESHOLD_CONNECTED_COMPONENT)
                {
                    CurrentState = 4;
                }
                else
                {
                    CurrentState=1;
                }
            }
            else if (CurrentState == 4)
            {
                //SetUtility for Speech to 1
                //CurrentState=1;
                this.personDetected = true;
                int directionInImage = this.webCamCurrentlyProcessingImage.MiddleOfLargestComponent;
                this.direcectionToPerson = this.cameraDirectionCurrentImage + directionInImage; // +15 => Vänster i bilden...
            }
        }
        #endregion

        #region Properties
        public Bitmap WebCamProcessedImage
        {
            get
            {
                if (webCamProcessedImage != null)
                {
                    return webCamProcessedImage;
                }
                else
                {
                    return new Bitmap(320, 240);
                }
            }
        }
        public bool PersonDetected
        {
            get { return this.personDetected; }
        }
        public double DirectionToPerson
        {
            get { return this.direcectionToPerson; }
        }
        public int PixelMiddleOfLargestComponent
        {
            get { return this.pixelMiddleOfLargestComponent; }
        }
        #endregion
    }
}
